\documentclass[10pt,a4paper]{article}
\usepackage{article}

\title{{\bf DD2425 Robotics \& Autonomous Systems}\\ \Large{Lab 1: Trajectory Control for a Robot Manipulator}}

\author{Emil Arfvidsson\\emilarf@kth.se
\and Gon\c{c}alo Rendeiro da Silva\\gards@kth.se
\and Matthijs Dorst\\dorst@kth.se
\and Samarth Deo\\samarthd@kth.se}

\date{\today}

\begin{document}
\maketitle

\section{Robot Specifications and parameters}
\subsection{Workspace}

\begin{itemize}
	\item What is the workspace for the robot?

\begin{figure}[h!]
	\begin{center}
 	\includegraphics[width=0.4\textwidth]{1_1_workspace.pdf}
	\end{center}
		\caption{Robot workspace overview} 
	\label{fig:workspace} 
\end{figure}

For each robot, the workspace can be described as a donut shaped area with outer radius $\alpha$ and inner radius $\beta$, as depicted in figure \ref{fig:workspace}. For the three robots, these values become:\\
\begin{center}
\begin{tabular}{lccc}
\toprule
	 & $\alpha$ & $\beta$ & area	($m^2$) \\
\cmidrule(r){2-4} 
Robot A & 0.8 & 0.0 & 2.01 \\
Robot B & 0.8 & 0.2 & 1.88 \\
Robot C & 0.8 & -0.2 & 1.88 \\
\bottomrule	 
\end{tabular}
\end{center}

	\item Are there any kinematic singularities?
		\begin{itemize}
			\item All robots: a kinematic singularity exists on the outer rim. Once the arm reaches any point on the outer rim, it cannot move further away from the center.
			\item All robots: another singularity exists when $\theta_2$ reaches $-45^{\circ}$. At this point, the end effector cannot move further inwards without moving outwards first.
			\item Robot B: another kinematic singularity exists on the inner rim, at a distance $\beta$ from the center.
		\end{itemize}
\end{itemize}


\subsection{Kinematics}
	\begin{itemize}
		\item Write down the function $K_f$ explicitly
	\end{itemize}
	
	\begin{eqnarray}
		X_{cart} & = &
			\left[ 
				\begin{array}{c}
					cos(\theta_1) \cdot L_1 + cos(\theta_1 + \theta_2) \cdot L_2 \\
					sin(\theta_1) \cdot L_1 + sin(\theta_1 + \theta_2) \cdot L_2 \\
					d_1 - L_3
				\end{array}
			\right]
	\end{eqnarray}


\subsection{Inverse Kinematics}

	\begin{itemize}
		\item 	How many different joint space solutions exist?

			\subitem 2 in general, except where the join space is constrained.

		\item Write down $K_i$

	\begin{eqnarray}
		X_{i} & = &
			\left[ 
				\begin{array}{c}
					atan2(x, y) \mp atan2(k, x^2 + y^2 + L_1^2 - L_2^2) \\
					\pm atan2(k, x^2 + y^2 - L_1^2 - L_2^2) \\
					d_1 - z
				\end{array}
			\right] \\
		k & = & \sqrt{(x^2 + y^2 + L_1^2 + L_2^2)^2 - 2[(x^2 + y^2)^2 + L_1^4 + L_2^4]}
	\end{eqnarray}

		\item 	What happens when the cartesian coordinates are a singularity point?

			\subitem The joint space solutions will have only one answer.

	\end{itemize}

\subsection{Velocity Jacobian}

	\begin{itemize}
		\item Calculate the Jacobian $J$.

		\begin{eqnarray}
			J(\Theta) & = &
			\left[
				\begin{matrix}
					-L_1\sin(\theta_1) - L_2\sin(\theta_1 + \theta_2) & -L_2\sin(\theta_1 + \theta_2) 	& 0 \\

					-L_1\cos(\theta_1) - L_2\cos(\theta_1 + \theta_2) & L_2\cos(\theta_1 + \theta_2)	& 0 \\

					0 & 0 & -1 \\
				\end{matrix}
			\right]
		\end{eqnarray}

		\item What happens to $J$ when the arm is in a singularity?
	
			\subitem	
			When the arm is at a singularity the velocity Jacobian will only be dependent of $\theta_1$, and the matrix will be linearly dependent (we only need one of the equations). This is perceivable by thinking the arm moving fully stretched or in the inner rim: the velocity of the arm will only depend on the variation of  $\theta_1$ related to time.
			
		\item How can we calculate $J_{inv}$ using only the expressions previously derived in (1.3) and (1.4)?
	
			\subitem 	The $J_{inv}$ can be calculated by calculating the Jacobian for the $K_i$ function.
	
		\item What happens to $J_{inv}$ when the arm is in a singularity?

				
		\begin{figure}[!ht]
			\begin{center}
		 	\includegraphics[width=0.8\textwidth]{1_4_J_inverse.pdf}
			\end{center}
				\caption{Inverse Jakobian in a singularity} 
			\label{fig:jakobian_singularity} 
		\end{figure}		

			\subitem When the arm is in a singularity, certain elements of $J_{inv}$ will be restricted in their possible values, see figure \ref{fig:jakobian_singularity}. At the outer singularity (circle A), the terms cannot obtain positive values in a straight outward direction (i.e., in a polar coordinate system, $\dot{r} \leq 0$).
		
			Conversely, at the middle and inner singularities (circles B and C) the terms cannot obtain negative values in a straight outward direction (i.e., in a polar coordinate system we would obtain $\dot{r} \geq 0$).	
		
		\item Which DoFs are coupled, and how does this affect complexity and the structure of $J$?

			\subitem	$\theta_1$ and $\theta_2$ are coupled. Since they are coupled they appear together in the terms in $J$, and increases the number of non-zero elements in the Jacobian.

	\end{itemize}

\section{Robot simulation}

	\subsection{Line welding}
		\begin{itemize}
			\item Your task is to generate a joint space trajectory that can be used in \texttt{robot\_sim} that fullfills these requirements. Run the functions without parameters to get definitions of inputs and outputs. Plot your trajectory as a function of time, both for cartesian space and joint space. Show these plots when you present your work.

				\begin{figure}[!ht]
					\begin{center}
				 	\includegraphics[width=0.8\textwidth]{2_1_plots.pdf}
					\end{center}
						\caption{Plots of cartesian space and joint space as a function of time}
					\label{fig:joint_space} 
				\end{figure}

		\end{itemize}

	\subsection{Performance}
		The time for the whole weld is divided in two parts - first part is positioning the head at the starting point and the second part is the line weld with the head down.
\\

		Time needed for the first part is the time needed for the joints to turn from initial position to start of weld position. The arm's initial position is [$60$ $-40$ $0.1$], and the start of weld position is [$118.29$ $100.81$ $0.2$]. $\Theta_1$ and $\Theta_2$ can turn at a speed of $50^{\circ}/$s in both directions, and the tip extends at $0.1$m/s. The time for the first part is the maximum of times needed for each joint. $\Theta_1$ needs 1.17s to rotate, $\Theta_2$ needs 2.82s to rotate and the tip needs 0.1s to extend fully. Therefore the first part cannot be done faster than 2.82s.

Because the tip does not need to bypass a singularity, nor suffers from any other constraints, the actual path is identical to this theoretical minimum, so 29 steps of 0.1s each are required in practice.
 \\
 
For the second part of the weld, an additional constraint is registered: the path of the weld must approximate a straight line. This complicates matters, as we can no longer assume the total path length equals the angular difference between start and end points. This is most clearly illustrated for $\Theta_2$ in figure \ref{fig:joint_space}: its start angle is roughly $100^{\circ}$ and its destination angle approximates $140^{\circ}$, yielding a path length of just $40^{\circ}$. However, at some point along its trajectory, it reaches a maximum angle of $150^{\circ}$, greater than the destination angle! To complicate matters further, its trajectory is not linear, much faster at some points and slower at others. Thus, it is possible to achieve an average speed of less than $50^{\circ}/$s, but still exceed this maximum speed at certain time-steps, which would invalidate the trajectory.

Luckily\footnote{And, this author believes, intentionally}, it seems that $\Theta_1$ does not suffer from any of these complications. As figure \ref{fig:joint_space} illustrates, its path is very nearly linear, such that a linear path assumption will hold in practice. And, because the path of $\Theta_1$ is much longer than that of $\Theta_2$, it is $\Theta_1$ which will impose a time requirement on us, allowing us to safely ignore the more complex path of $\Theta_2$. 

Calculating the difference between start and end point of $\Theta_1$'s path, we find a total path length of slightly more than $130^{\circ}$. At the given maximum velocity, this requires about 27 time-steps to traverse. In practice, having so few points along the weld induces a deviation from the desired path that exceeds the imposed restrictions, and only with 29 time steps is the weld accepted.


\end{document}











